#include "spellogica.h"
#define _USE_MATH_DEFINES
#include <math.h>
#include <stdlib.h>

// vec2 type definition and functions
typedef struct vec2 {
	float x,y;
} vec2;
vec2 vec2Norm(vec2 v) {
	float d = v.x*v.x + v.y*v.y;
	if(d > 0) {
		d = sqrt(d);
		v.x /= d;
		v.y /= d;
		return v;
	}
	return (vec2){0,0};
}
vec2 vec2From(float x, float y) {
	return (vec2){x,y};
}
vec2 vec2Add(vec2 v, vec2 w) {
	return (vec2){v.x+w.x,v.y+w.y};
}
vec2 vec2Sub(vec2 v, vec2 w) {
	return (vec2){v.x-w.x,v.y-w.y};
}
vec2 vec2Scale(vec2 v, float s) {
	return (vec2){v.x*s,v.y*s};
}
float vec2Len(vec2 v) {
	float d = v.x*v.x+v.y*v.y;
	if(d > 0) {
		d = sqrt(d);
		return d;
	}
	return 0;
}
float vec2Dot(vec2 v, vec2 w) {
	return v.x*w.x+v.y*w.y;
}
float vec2Angle(vec2 v) {
	return atan2(v.y,v.x);
}


// Global variables
vec2 ballPos;
vec2 ballVel;
vec2 gravity;
int winstate;

// Collision map size
// Should always be uneven*uneven and greater than or equal to 1.
int colMapWidth = 7;
int colMapHeight = 7;

// Speeds greater than this number will cause the game logic to run at multiple ticks per second, with a lower ballspeed.
// This will make the speed of the ball never exceed the size of a pixel.
float MAX_SPEED = 0.707070;
// Upon collision with speeds lower than this number, this will be the assumed speed.
// This will prevent a ball slowly phasing through solid objects.
float MIN_SPEED = 0.8;
// This is the speed multiplier for when the ball collides with the wall.
float COLLISION_SPEED_MUL =	0.95;
// Speed of gravity.
float GRAVITY_SPEED = 0.04;
// Wind resistance
float WIND_RESISTANCE = 0.985;
// Distance at which the ball counts as finished
float FINISH_DISTANCE = 20.0;
// Speed the ball gains on top of gravity when the ball has finished.
float WIN_SPEED = -0.04;

// map the ball moves in.
void* map = NULL; // RGB888 format (24bit/px).
int mapWidth;
int mapHeight;
int mapStartX;
int mapStartY;
int mapEndX;
int mapEndY;

// ball itself.
void* ball = NULL;
int ballWidth;
int ballHeight;


// spellogica functions

// Handles velocity of the ball.
void handleVelocity() {
	ballVel = vec2Add(ballVel,gravity);
	ballVel = vec2Scale(ballVel,WIND_RESISTANCE);
}

// Moves the ball. The movement vector is ballVel*mul.
void moveBall(float mul) {
	vec2 ballVel2 = vec2Scale(ballVel,mul);
	ballPos = vec2Add(ballPos,ballVel2);
}

// Performs bounds checking and forces the ball inside the map.
void boundsCheck() {
	if(ballPos.x < ballWidth+colMapWidth) {
		ballPos.x = ballWidth+colMapWidth;
		ballVel.x = -ballVel.x*COLLISION_SPEED_MUL;
	} else if(ballPos.x > mapWidth-ballWidth-colMapWidth) {
		ballPos.x = mapWidth-ballWidth-colMapWidth;
		ballVel.x = -ballVel.x*COLLISION_SPEED_MUL;
	}
	if(ballPos.y < ballHeight+colMapHeight) {
		ballPos.y = ballHeight+colMapHeight;
		ballVel.y = -ballVel.y*COLLISION_SPEED_MUL;
	} else if(ballPos.y > mapHeight-ballHeight-colMapHeight) {
		ballPos.y = mapHeight-ballHeight-colMapHeight;
		ballVel.y = -ballVel.y*COLLISION_SPEED_MUL;
	}
}

// Returns pixel x,y from the given pixels with size width,height, in 0x00RRGGBB format.
unsigned int fetchPixel(void* pixels, unsigned int width, unsigned int height, unsigned int x, unsigned int y) {
	if(x >= width || y >= height) return 0;
	struct pixel {
		char r,g,b;
	};
	struct pixel* p = (struct pixel*) pixels;
	struct pixel px = p[x+y*width];
	int i = 0;
	i += px.r << 16;
	i += px.g << 8;
	i += px.b << 0;
	return i;
}

// Returns color of given map position in 0x00RRGGBB format
unsigned int getMapPos(unsigned int x, unsigned int y) {
	return fetchPixel(map,mapWidth,mapHeight,x,y);
}

// Returns color of the given ball position in 0x00RRGGBB format
unsigned int getBallPos(unsigned int x, unsigned int y) {
	return fetchPixel(ball,ballWidth,ballHeight,x,y);
}

// Returns 1 upon collision, with colPos set to the colliding pixel.
int collisionCheck(int* colPosX, int* colPosY) {
	float dist = 1000; // this can be any high number
	for(int y = 0; y<ballHeight; y++) {
		int mapY = ballPos.y + y - ballHeight/2;
		for(int x = 0; x<ballWidth; x++) {
			int mapX = ballPos.x + x - ballWidth/2;
			int pxBall = getBallPos(x,y);
			if(pxBall != 0x00FFFFFF) {
				int pxMap = getMapPos(mapX,mapY);
				if(pxMap == 0x00000000) {
					float xDist = abs(ballWidth-x);
					float yDist = abs(ballWidth-y);
					float d = xDist*xDist + yDist*yDist;
					d = sqrt(d);
					if(d < dist) {
						dist = d;
						*colPosX = mapX;
						*colPosY = mapY;
					}
				}
			}
		}
	}
	if(dist < 1000) return 1;
	return 0;
}

// Returns the facing vector of the wall in the given colMap
vec2 getWallVec(int* colMap, int w, int h) {
	vec2 v = vec2From(0,0);
	float len = sqrt((w/2)*(w/2) + (h/2)*(h/2));
	for(int x = 0; x<w; x++) {
		for(int y = 0; y<h; y++) {
			if(colMap[x+y*w] == 1) {
				vec2 v2 = vec2From(x-w/2,y-h/2);
				float vLen = vec2Len(v2);
				v2 = vec2Norm(v2);
				v2 = vec2Scale(v2,-1 * (len - vLen));
				v = vec2Add(v,v2);
			}
		}
	}
	v = vec2Norm(v);
	return v;
}

// Functions as defined by spellogica.h

// Run game logic. Returns 0 when there are no errors.
// Passed float is the orientation of the gravity.
int runSpelLogica(float orientation) {
	if(map == NULL) return -1;
	gravity = (vec2){cos(orientation),sin(orientation)};
	gravity = vec2Scale(gravity,GRAVITY_SPEED+winstate*WIN_SPEED);
	handleVelocity();
	float steprate = 1.0;
	int steps = 1;
	if(vec2Len(ballVel) > MAX_SPEED) {
		steps = 1 + (vec2Len(ballVel) / MAX_SPEED);
		steprate = steprate/(float)steps;
	}
	for(int i = 0; i<steps; i++) {
		moveBall(steprate);
		boundsCheck();
		int xCol, yCol;
		if(collisionCheck(&xCol,&yCol)) {
			// Construct collision map for calculating the direction the wall is facing.
			int colMap[colMapWidth*colMapHeight];
			for(int y = 0; y<colMapHeight; y++) {
				for(int x = 0; x<colMapWidth; x++) {
					int px = getMapPos(x+ballPos.x-colMapWidth/2,y+ballPos.y-colMapHeight/2);
					if(px == 0x00000000) {
						colMap[x+y*colMapWidth] = 1;
					} else {
						colMap[x+y*colMapWidth] = 0;
					}
				}
			}

			// Required data for calculation
			vec2 vWall = getWallVec(colMap,colMapWidth,colMapHeight);
			float aW = vec2Angle(vWall);
			float aB = vec2Angle(ballVel);
			float speed = vec2Len(ballVel);

			// Turn ball vector so it's in respect to the wall vector (angle = 0 == straight away from wall)
			aB -= aW;
			if(aB < -M_PI) aB += 2*M_PI;
			if(aB > M_PI) aB -= 2*M_PI;

			// Flip ball angle to negative
			aB = -aB;

			// Turn ball angle back to real situation, and turn it M_PI rad to go away from the wall.
			aB += aW + M_PI;
			if(aB < -M_PI) aB += 2*M_PI;
			if(aB > M_PI) aB -= 2*M_PI;

			// Reconstruct ball vector with new angle, and adjust speed accordingly.
			ballVel = vec2From(cos(aB)*speed,sin(aB)*speed);
			ballVel = vec2Scale(ballVel,COLLISION_SPEED_MUL);

			// If the ball, for whatever reason, still attempts to go towards the wall, turn it around.
			float dot = vec2Dot(ballVel, vWall);
			if(dot < 0) {
				ballVel = vec2Scale(ballVel,-1);
			}

			// Apply minimum speed to prevent the ball from phasing through the wall.
			if(vec2Len(ballVel) < MIN_SPEED) {
				float a = vec2Angle(ballVel);
				ballVel = vec2From(cos(a)*MIN_SPEED,sin(a)*MIN_SPEED);
			}
		}
	}
	vec2 diff = vec2Sub(ballPos,(vec2){mapEndX,mapEndY});
	if(vec2Len(diff) < FINISH_DISTANCE) {
		winstate = 1;
	}
	if(winstate) return winstate;
	return 0;
}

// Returns the top left position of the ball.
int getBallX(void) {
	return ballPos.x - ballWidth/2;
}

int getBallY(void) {
	return ballPos.y - ballWidth/2;
}

// Returns the ball vector
float getBallVectorX(void) {
	return ballVel.x;
}

float getBallVectorY(void) {
	return ballVel.y;
}

// Sets the map to play on.
// Pixels are in RGB888 format (24bit/px).
void setSpeelveld(
	void* pixels,
	unsigned int width,
	unsigned int height,
	unsigned int startX,
	unsigned int startY,
	unsigned int endX,
	unsigned int endY
) {
	map = pixels;
	mapWidth = width;
	mapHeight = height;
	mapStartX = startX;
	mapStartY = startY;
	mapEndX = endX;
	mapEndY = endY;
	ballPos = (vec2){startX,startY};
	winstate = 0;
}

// Sets the ball image. Pixels are in RGB888 format (24bit/px).
// The ball dimensions should be uneven and equal. (e.g. 7x7)
void setBall(void* pixels, unsigned int width, unsigned int height) {
	ball = pixels;
	ballWidth = width;
	ballHeight = height;
}


